
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       gcs_param.c
  * @author     baiyang
  * @date       2022-11-9
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "gcs.h"

#include <float.h>
#include <string.h>

#include <rtthread.h>

#include <logger/ap_log.h>
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// return a MAVLink parameter type given a AP_Param type
MAV_PARAM_TYPE gcsmav_mav_param_type(enum param_type_t t)
{
    if (t == PARAM_TYPE_INT8) {
        return MAV_PARAM_TYPE_INT8;
    }

    if (t == PARAM_TYPE_UINT8) {
        return MAV_PARAM_TYPE_UINT8;
    }

    if (t == PARAM_TYPE_INT16) {
        return MAV_PARAM_TYPE_INT16;
    }

    if (t == PARAM_TYPE_UINT16) {
        return MAV_PARAM_TYPE_UINT16;
    }

    if (t == PARAM_TYPE_INT32) {
        return MAV_PARAM_TYPE_INT32;
    }

    if (t == PARAM_TYPE_UINT32) {
        return MAV_PARAM_TYPE_UINT32;
    }
    // treat any others as float
    return MAV_PARAM_TYPE_REAL32;
}

/**
 * @brief Send the next pending parameter, called from deferred message
 * handling code
 */
void gcsmav_queued_param_send(gcs_mavlink_t gcsmav)
{
    // send parameter async replies
    uint8_t async_replies_sent_count = 0;

    // now send the streaming parameters (from PARAM_REQUEST_LIST)
    if (gcsmav->_queued_parameter == NULL) {
        // .... or not....
        return;
    }

    const uint32_t tnow = time_millis();
    const uint32_t tstart = time_micros();

    // use at most 30% of bandwidth on parameters
    const uint32_t link_bw = SerialManager_bw_in_bytes_per_second(gcsmav->_port);

    uint32_t bytes_allowed = link_bw * (tnow - gcsmav->_queued_parameter_send_time_ms) / 3333;
    const uint16_t size_for_one_param_value_msg = MAVLINK_MSG_ID_PARAM_VALUE_LEN + gcsmav_packet_overhead(gcsmav);
    if (bytes_allowed < size_for_one_param_value_msg) {
        bytes_allowed = size_for_one_param_value_msg;
    }
    if (bytes_allowed > gcsmav_txspace(gcsmav)) {
        bytes_allowed = gcsmav_txspace(gcsmav);
    }
    uint32_t count = bytes_allowed / size_for_one_param_value_msg;

    // when we don't have flow control we really need to keep the
    // param download very slow, or it tends to stall
    if (!gcsmav_have_flow_control(gcsmav) && count > 5) {
        count = 5;
    }
    if (async_replies_sent_count >= count) {
        return;
    }
    count -= async_replies_sent_count;

    while (count && gcsmav->_queued_parameter != NULL && gcsmav->last_txbuf > 50) {
        char param_name[PARAM_MAX_NAME_SIZE + 1] = {0};
        rt_memcpy(param_name, gcsmav->_queued_parameter->name, PARAM_MAX_NAME_SIZE);

        mavlink_msg_param_value_send(
            gcsmav->chan,
            param_name,
            param_cast_to_float(gcsmav->_queued_parameter),
            gcsmav_mav_param_type((enum param_type_t)gcsmav->_queued_parameter->type),
            gcsmav->_queued_parameter_count,
            gcsmav->_queued_parameter_index);

        gcsmav->_queued_parameter = param_get_next_scalar(gcsmav->_queued_parameter);
        gcsmav->_queued_parameter_index++;

        if (time_micros() - tstart > 1000) {
            // don't use more than 1ms sending blocks of parameters
            break;
        }
        count--;
    }
    gcsmav->_queued_parameter_send_time_ms = tnow;
}

/*
  return true if a channel has flow control
 */
bool gcsmav_have_flow_control(gcs_mavlink_t gcsmav)
{
    if (gcsmav->_port == NULL) {
        return false;
    }

#if 0
    if (gcsmav->_port->get_flow_control() != AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE) {
        return true;
    }
#endif

    if (gcsmav->chan == MAVLINK_COMM_0) {
        // assume USB console has flow control
        return SerialManager_usb_connected();
    }

    return false;
}

/*
  handle a request to change stream rate. Note that copter passes in
  save==false so we don't want the save to happen when the user connects the
  ground station.
 */
void gcsmav_handle_request_data_stream(gcs_mavlink_t gcsmav, const mavlink_message_t *msg)
{
    mavlink_request_data_stream_t packet;
    mavlink_msg_request_data_stream_decode(msg, &packet);

    int16_t freq = 0;     // packet frequency

    if (packet.start_stop == 0)
        freq = 0;                     // stop sending
    else if (packet.start_stop == 1)
        freq = packet.req_message_rate;                     // start sending
    else
        return;

    param_t* param = NULL;

    // if stream_id is still NUM_STREAMS at the end of this switch
    // block then either we set stream rates for all streams, or we
    // were asked to set the streamrate for an unrecognised stream
    enum mav_streams stream_id = NUM_STREAMS;
    switch (packet.req_stream_id) {
    case MAV_DATA_STREAM_ALL:
        for (uint8_t i=0; i<NUM_STREAMS; i++) {
            if (i == STREAM_PARAMS) {
                // don't touch parameter streaming rate; it is
                // considered "internal".
                continue;
            }

            if (gcsmav_persist_streamrates(gcsmav)) {
                param = param_get_by_variable2("GCS", (void*)&gcsmav->streamRates[i]);
                if (param != NULL) {
                    param_set_and_save_ifchanged(param, freq);
                }
            } else {
                gcsmav->streamRates[i] = freq;
            }

            gcsmav_initialise_message_intervals_for_stream(gcsmav, (enum mav_streams)i);
        }
        break;
    case MAV_DATA_STREAM_RAW_SENSORS:
        stream_id = STREAM_RAW_SENSORS;
        break;
    case MAV_DATA_STREAM_EXTENDED_STATUS:
        stream_id = STREAM_EXTENDED_STATUS;
        break;
    case MAV_DATA_STREAM_RC_CHANNELS:
        stream_id = STREAM_RC_CHANNELS;
        break;
    case MAV_DATA_STREAM_RAW_CONTROLLER:
        stream_id = STREAM_RAW_CONTROLLER;
        break;
    case MAV_DATA_STREAM_POSITION:
        stream_id = STREAM_POSITION;
        break;
    case MAV_DATA_STREAM_EXTRA1:
        stream_id = STREAM_EXTRA1;
        break;
    case MAV_DATA_STREAM_EXTRA2:
        stream_id = STREAM_EXTRA2;
        break;
    case MAV_DATA_STREAM_EXTRA3:
        stream_id = STREAM_EXTRA3;
        break;
    }

    if (stream_id == NUM_STREAMS) {
        // asked to set rate on unknown stream (or all were set already)
        return;
    }

    Param_int16* rate = &gcsmav->streamRates[stream_id];

    if (rate != NULL) {
        if (gcsmav_persist_streamrates(gcsmav)) {
            param = param_get_by_variable2("GCS", (void*)rate);
            if (param != NULL) {
                param_set_and_save_ifchanged(param, freq);
            }
        } else {
            *rate = freq;
        }

        gcsmav_initialise_message_intervals_for_stream(gcsmav, stream_id);
    }
}

void gcsmav_handle_param_request_list(gcs_mavlink_t gcsmav, const mavlink_message_t *msg)
{
    if (!gcsmav_params_ready(gcsmav)) {
        return;
    }

    mavlink_param_request_list_t packet;
    mavlink_msg_param_request_list_decode(msg, &packet);

    // requesting parameters is a convenient way to get extra information
    gcsmav_send_banner(gcsmav);

    // Start sending parameters - next call to ::update will kick the first one out
    gcsmav->_queued_parameter = param_get_first();
    gcsmav->_queued_parameter_index = 0;
    gcsmav->_queued_parameter_count = param_get_count();
    gcsmav->_queued_parameter_send_time_ms = time_millis(); // avoid initial flooding
}

void gcsmav_handle_param_request_read(gcs_mavlink_t gcsmav, const mavlink_message_t *msg)
{
    mavlink_param_request_read_t packet;
    mavlink_msg_param_request_read_decode(msg, &packet);

    /*
      we reserve some space for sending parameters if the client ever
      fails to get a parameter due to lack of space
     */
    uint32_t saved_reserve_param_space_start_ms = gcs_reserve_param_space_start_ms;
    gcs_reserve_param_space_start_ms = 0; // bypass packet_overhead_chan reservation checking
    if (!MAV_HAVE_PAYLOAD_SPACE(gcsmav->chan, PARAM_VALUE)) {
        gcs_reserve_param_space_start_ms = time_millis();
        return;
    } else {
        gcs_reserve_param_space_start_ms = saved_reserve_param_space_start_ms;
    }

    param_t* vp;
    char param_name[PARAM_MAX_NAME_SIZE+1] = {0};

    if (packet.param_index != -1) {
        vp = param_get_by_index(packet.param_index);
        if (vp == NULL) {
            return;
        }
        rt_memcpy(param_name, vp->name, MIN(rt_strlen(vp->name), PARAM_MAX_NAME_SIZE));
    } else {
        rt_memcpy(param_name, packet.param_id, PARAM_MAX_NAME_SIZE);
        vp = param_get_by_name(param_name);
        if (vp == NULL) {
            return;
        }
    }

    mavlink_msg_param_value_send(
            gcsmav->chan,
            param_name,
            param_cast_to_float(vp),
            gcsmav_mav_param_type((enum param_type_t)vp->type),
            param_get_count(),
            packet.param_index);

    gcsmav->_queued_parameter_send_time_ms = time_millis();
}

void gcsmav_handle_param_set(gcs_mavlink_t gcsmav, const mavlink_message_t *msg)
{
    mavlink_param_set_t packet;
    mavlink_msg_param_set_decode(msg, &packet);

    // set parameter
    param_t* vp;
    char key[PARAM_MAX_NAME_SIZE+1];
    strncpy(key, (char *)packet.param_id, PARAM_MAX_NAME_SIZE);
    key[PARAM_MAX_NAME_SIZE] = 0;

    // find existing param so we can get the old value
    vp = param_get_by_name(key);
    if (vp == NULL || isnan(packet.param_value) || isinf(packet.param_value)) {
        return;
    }

    // set the value
    param_set_float(vp, packet.param_value);
    param_save_float(vp);

    aplog_write_parameter(key, param_cast_to_float(vp), vp->default_val);
}

void gcsmav_send_parameter_value(gcs_mavlink_t gcsmav, const char *param_name, enum param_type_t param_type, float param_value)
{
    if (!MAV_HAVE_PAYLOAD_SPACE(gcsmav->chan, PARAM_VALUE)) {
        return;
    }

    mavlink_msg_param_value_send(
        gcsmav->chan,
        param_name,
        param_value,
        gcsmav_mav_param_type(param_type),
        param_get_count(),
        -1);
}

void gcsmav_handle_common_param_message(gcs_mavlink_t gcsmav, const mavlink_message_t *msg)
{
    switch (msg->msgid) {
    case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
        gcsmav_handle_param_request_list(gcsmav, msg);
        break;
    case MAVLINK_MSG_ID_PARAM_SET:
        gcsmav_handle_param_set(gcsmav, msg);
        break;
    case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
        gcsmav_handle_param_request_read(gcsmav, msg);
        break;
    }
}

/*------------------------------------test------------------------------------*/


